import lejos.nxt.*;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.*;

public class NoLeftWall implements Behavior {
    private UltrasonicSensor sonar;
    private DifferentialPilot pilot = new DifferentialPilot(28f, 105f, Motor.C, Motor.A, false);
    private boolean suppressed = false;
    
    public NoLeftWall(SensorPort port)
    {
       sonar = new UltrasonicSensor(port);
    }

    public boolean takeControl() {
       return (sonar.getDistance() > 50) && (Motor.B.getTachoCount() < 0);
    }

    public void suppress() {
       suppressed = true;
    }

    public void action() {
       suppressed = false;
       LCD.clear(3);
       LCD.drawString("Returning sonar", 0, 3);
       Motor.B.rotateTo(0, true);
       Motor.B.resetTachoCount();

       while( Motor.C.isMoving() && !suppressed )
         Thread.yield();

       Motor.B.stop();
       
       LCD.clear(3);
       LCD.drawString("Turning left", 0, 3);
       pilot.rotate(-90);
       
       while(pilot.isMoving() && !suppressed)
           Thread.yield();  // wait till turn is complete or suppressed is called
       
       pilot.stop();
    }
}